#include "motor.h"
#include "stm32f4xx_hal_can.h"
int a=0;
moto_info_t motor_info_GM6020[7];
moto_info_t motor_info_M3508_M2006[7];

void set_motor_voltage_M2006(uint8_t id_range, int16_t v1, int16_t v2, int16_t v3, int16_t v4)
{
  CAN_TxHeaderTypeDef tx_header;
  uint8_t             tx_data[8];
    
  tx_header.StdId = (id_range == 0)?(0x200):(0x1ff);
  tx_header.IDE   = CAN_ID_STD;
  tx_header.RTR   = CAN_RTR_DATA;
  tx_header.DLC   = 8;

  tx_data[0] = (v1>>8)&0xff;
  tx_data[1] =    (v1)&0xff;
  tx_data[2] = (v2>>8)&0xff;
  tx_data[3] =    (v2)&0xff;
  tx_data[4] = (v3>>8)&0xff;
  tx_data[5] =    (v3)&0xff;
  tx_data[6] = (v4>>8)&0xff;
  tx_data[7] =    (v4)&0xff;
  HAL_CAN_AddTxMessage(&hcan1, &tx_header, tx_data,(uint32_t*)CAN_TX_MAILBOX0); 
	
}

void set_motor_voltage_M3508(uint8_t id_range, int16_t v1, int16_t v2, int16_t v3, int16_t v4)
{
  CAN_TxHeaderTypeDef tx_header;
  uint8_t             tx_data[8];
    
  tx_header.StdId = (id_range == 0)?(0x200):(0x1ff);
  tx_header.IDE   = CAN_ID_STD;
  tx_header.RTR   = CAN_RTR_DATA;
  tx_header.DLC   = 8;

  tx_data[0] = (v1>>8)&0xff;
  tx_data[1] =    (v1)&0xff;
  tx_data[2] = (v2>>8)&0xff;
  tx_data[3] =    (v2)&0xff;
  tx_data[4] = (v3>>8)&0xff;
  tx_data[5] =    (v3)&0xff;
  tx_data[6] = (v4>>8)&0xff;
  tx_data[7] =    (v4)&0xff;
  HAL_CAN_AddTxMessage(&hcan1, &tx_header, tx_data,(uint32_t*)CAN_TX_MAILBOX0); 
	
}
void set_motor_voltage_GM6020_pitch(uint8_t id_range, int16_t v1, int16_t v2, int16_t v3, int16_t v4)
{
  CAN_TxHeaderTypeDef tx_header;
  uint8_t             tx_data[8];
    
  tx_header.StdId = (id_range == 0)?(0x1ff):(0x2ff);
  tx_header.IDE   = CAN_ID_STD;
  tx_header.RTR   = CAN_RTR_DATA;
  tx_header.DLC   = 8;

  tx_data[0] = (v1>>8)&0xff;
  tx_data[1] =    (v1)&0xff;
  tx_data[2] = (v2>>8)&0xff;
  tx_data[3] =    (v2)&0xff;
  tx_data[4] = (v3>>8)&0xff;
  tx_data[5] =    (v3)&0xff;
  tx_data[6] = (v4>>8)&0xff;
  tx_data[7] =    (v4)&0xff;
  HAL_CAN_AddTxMessage(&hcan1, &tx_header, tx_data,(uint32_t*)CAN_TX_MAILBOX0); 
}

void set_motor_voltage_GM6020_yaw(uint8_t id_range, int16_t v1, int16_t v2, int16_t v3, int16_t v4)
{
  CAN_TxHeaderTypeDef tx_header;
  uint8_t             tx_data[8];
    
  tx_header.StdId = (id_range == 0)?(0x1ff):(0x2ff);
  tx_header.IDE   = CAN_ID_STD;
  tx_header.RTR   = CAN_RTR_DATA;
  tx_header.DLC   = 8;

  tx_data[0] = (v1>>8)&0xff;
  tx_data[1] =    (v1)&0xff;
  tx_data[2] = (v2>>8)&0xff;
  tx_data[3] =    (v2)&0xff;
  tx_data[4] = (v3>>8)&0xff;
  tx_data[5] =    (v3)&0xff;
  tx_data[6] = (v4>>8)&0xff;
  tx_data[7] =    (v4)&0xff;
  HAL_CAN_AddTxMessage(&hcan2, &tx_header, tx_data,(uint32_t*)CAN_TX_MAILBOX0); 
}

void Set_chassis(uint8_t id_range, int16_t y_speed, int16_t x_speed , int16_t w_speed, uint8_t mode , uint8_t command )
{
  CAN_TxHeaderTypeDef tx_header;
	uint8_t             tx_data[8];
	
	tx_header.StdId = (id_range == 0)?(0x222):(0x1ff); 
	tx_header.IDE = CAN_ID_STD;
	tx_header.RTR = CAN_RTR_DATA;
	tx_header.DLC = 8;
	
	tx_data[0] = (y_speed>>8)&0xff; 
  tx_data[1] =    (y_speed)&0xff;
  tx_data[2] = (x_speed>>8)&0xff;
  tx_data[3] =    (x_speed)&0xff;
  tx_data[4] = (w_speed>>8)&0xff;
  tx_data[5] =    (w_speed)&0xff;
  tx_data[6] =      (mode) &0xff;
  tx_data[7] =    (command)&0xff;
  HAL_CAN_AddTxMessage(&hcan2, &tx_header, tx_data,(uint32_t*)CAN_TX_MAILBOX1);
}

void delay_us(uint16_t xus)
{
	for(uint16_t i = xus;i > 0;i--)
	{
		for(uint8_t j = 0;j < 168;j++);
	}
}

void delay_ms(uint16_t xms)
{
	for(uint16_t i = xms;i > 0;i--)
	{
		delay_us(1000);
	}
}
float  motor_round_M2006(int16_t angle_new)
{ 
static int n=0;
static int16_t angle_last;
static uint8_t init_flag = 1;
	float angle_abs = 0;
	
	if(init_flag)
	{
		init_flag = 0;
		n = 0;
	}
	else{

   if(angle_new-angle_last<-4096)
	 {
		 n++;
	 }
	 if(angle_new-angle_last>4096)
	 {
		 n--;
	 }
 }
		 angle_abs =angle_new+n*8192;

				angle_last = angle_new;	 
	 return angle_abs ;
}

float  motor_round_GM6020(float  angle_new)
{ 

static float  angle_last;
static uint8_t init_flag = 1;
	float angle_abs = 0;
	
	if(init_flag)
	{
		init_flag = 0;
		a = 0;
	}
	else{

   if(angle_new-angle_last<-180)
	 {
		 a++;
	 }
	 if(angle_new-angle_last>180)
	 {
		 a--;
	 }
 }
		 angle_abs =angle_new+a*360;

				angle_last = angle_new;	 
	 return angle_abs ;
}

float motor_current(uint16_t current)
{
	if(current<6700)
	{
		current =6700;
	
  }
	if(current>7900)
	{
		current=7900;
		
	}
	return current ;
}
